# import rospy
# import serial
# import struct
# from sensor_msgs.msg import JointState

# class MasterArmController:
#     def __init__(self):
#         rospy.init_node('puppet_arm_controller', anonymous=True)
#         self.sub = rospy.Subscriber('/master/joint_left', JointState, self.callback)
#         # self.pub = rospy.Publisher('/master/joint_left', JointState, queue_size=10)
#         self.offsets = [0, 0, 0, 0, 0, 0]  # 偏置值，根据实际情况调整

#         # 初始化串口
#         self.ser = serial.Serial(port="/dev/ttyUSB0", baudrate=9600, timeout=1)
#         if self.ser.isOpen():
#             print("Serial port opened successfully")
#         else:
#             print("Failed to open serial port")

#     def callback(self, data):
#         # 计算大臂的位置
#         puppet_arm_position = [p + o for p, o in zip(data.position, self.offsets)]
#         # puppet_arm_position = [1,2,3,4,5,6]
#         # 创建并发布 JointState 消息
#         msg = JointState()
#         msg.header.stamp = rospy.Time.now()
#         msg.name = ["puppet_joint1", "puppet_joint2", "puppet_joint3", "puppet_joint4", "puppet_joint5", "puppet_joint6"]
#         msg.position = puppet_arm_position
#         # msg.velocity = []
#         # msg.effort = []
#         self.pub.publish(msg)
#         rospy.loginfo("Published puppet arm position: %s", msg.position)

#         # 发送数据到STM32
#         self.send_to_stm32(puppet_arm_position)

#     def send_to_stm32(self, positions):
#         # 检查位置数据的长度是否为6
#         if len(positions) != 6:
#             rospy.logerr("Expected 6 position values, but got %d", len(positions))
#             return
#         # 将位置数据打包为字节流
#         # data = struct.pack('<6f', *positions)
#         frame_format = '<B6f'
#         # frame_header = b'\xA5'
#         frame_header = 0xA5
#         # frame_footer = b'\x55'
#         # data_length = struct.pack('B',len(data))
#         # checksum = sum(data) & 0xFF
#         # frame = frame_header + data
#         frame = struct.pack(frame_format,frame_header,*positions)
#         self.ser.write(frame)
#         # self.ser.write(data)
#         rospy.loginfo("Sent to STM32: %s", frame)
#         print("send to stm32 H:",frame.hex())

#     def run(self):
#         rate = rospy.Rate(10)  # 10 Hz
#         while not rospy.is_shutdown():
#             rate.sleep()

# if __name__ == '__main__':
#     controller = MasterArmController()
#     try:
#         controller.run()
#     except rospy.ROSInterruptException:
#         pass
#     finally:
#         controller.ser.close()
#         print("Serial port closed")


import rospy
import serial
import struct
from sensor_msgs.msg import JointState

class MasterArmController:
    def __init__(self):
        rospy.init_node('master_controller', anonymous=True)
        
        # 可配置参数
        self.joint_count = 6  # 期望的关节数量
        self.serial_port = rospy.get_param('~port', '/dev/ttyUSB0')
        self.baudrate = rospy.get_param('~baudrate', 9600)
        
        try:
            self.ser = serial.Serial(
                port=self.serial_port,
                baudrate=self.baudrate,
                timeout=1,
                bytesize=serial.EIGHTBITS,
                parity=serial.PARITY_NONE,
                stopbits=serial.STOPBITS_ONE
            )
            rospy.loginfo("Serial port opened successfully")
        except serial.SerialException as e:
            rospy.logerr("Failed to open serial port: %s", e)
            rospy.signal_shutdown("Serial port not opened")
            return
            
        self.sub = rospy.Subscriber('/master/joint_left', JointState, self.callback)
        rospy.loginfo("Node initialized and ready")

    def callback(self, data):
        if len(data.position) != self.joint_count:
            rospy.logwarn("Received %d joints, expected %d", len(data.position), self.joint_count)
            return
            
        rospy.loginfo("Adjusted joint positions: %s", data.position)
        
        try:
            # 使用小端字节序打包数据
            packed_data = struct.pack('>%df' % self.joint_count, *data.position)
            self.ser.write(packed_data)
            rospy.logdebug("Data sent to serial port")
        except Exception as e:
            rospy.logerr("Error sending data to serial port: %s", e)
            self.ser.close()
            rospy.signal_shutdown("Serial communication error")

if __name__ == '__main__':
    try:
        controller = MasterArmController()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass
    finally:
        if hasattr(controller, 'ser') and controller.ser.isOpen():
            controller.ser.close()
            rospy.loginfo("Serial port closed")